/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_error.h"
#include "zino.h"
#include "usercmd.h"
#include <rtthread.h>
#define DBG_TAG "rgb_switch"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define COLOR_NUM 7
#define SHOW_TIME 120
#define MAX_DIN 255

void mixly_rgb_mode_handle(void);
enum MIXLY_RGB_MODE
{
    MIXLY_RGB_MODE_IDLE = 0,
    MIXLY_RGB_BREATH,
    MIXLY_RGB_FAST,
    MIXLY_RGB_SLOW,
    MIXLY_RGB_DOT,
    MIXLY_RGB_COUNT
};
uint8_t mixlyBlinkTable[MIXLY_RGB_COUNT];
uint8_t mixlyRgbMode1 = 0;
uint8_t mixlyRgbMode2 = 0;
uint8_t mixlyMainRgb[3] = {0};
uint8_t mixlyTailRgb[3] = {0};

uint32_t fc_error_flag = 0;

const uint8_t color_list[COLOR_NUM][3] = {
    // 白色
    {255, 255, 255},
    // 绿色
    {0, 255, 0},
    // 红色
    {255, 0, 0},
    // 黄色
    {255, 255, 0},
    // 蓝色
    {0, 0, 255},
    // 紫色
    {255, 0, 255},
    // 青色
    {0, 255, 255},
};
enum rgb_mode
{
    RGB_MODE_IDLE = 0,
    RGB_MODE_1, // main rgb flash cycling, tail rgb change after press
    RGB_MODE_2, // main all rgb rgb change after press
    RGB_MODE_COUNT,
};

static uint8_t rgb_mode = RGB_MODE_IDLE;
static uint8_t rgb_Index = 0;
static uint8_t flagCnt = 0;
static uint8_t interval_cnt = 0;
static uint8_t breathCnt = 0;
static uint8_t breathTime = 100;
uint8_t r, g, b;
uint8_t mainRgb[3];
uint8_t tailRgb[3];
uint8_t manaulModeRgb[3];
uint8_t flitingRgb[3];
#define FLASH_TIME 25
void rgb_mode_switch_fun(bool triger)
{
    if (FC_ERROR_CHECK(FC_ARMED) || FC_ERROR_CHECK(FC_OK_TO_ARM) || FC_ERROR_CHECK(FC_ARMING_ALERK))
    {
        interval_cnt = 250;
    }
    if (triger && interval_cnt == 0)
    {
        switch (rgb_mode)
        {
        case RGB_MODE_IDLE:
            rgb_Index = 0;
            flagCnt = 0;
            rgb_mode++;
            break;
        case RGB_MODE_1:
            rgb_Index++;
            flagCnt = 0;
            if (rgb_Index >= COLOR_NUM)
            {
                rgb_Index = 0;
                rgb_mode++;
            }
            break;
        case RGB_MODE_2:
            rgb_Index++;

            flagCnt = 0;
            if (rgb_Index >= COLOR_NUM)
            {
                rgb_Index = 0;
                rgb_mode++;
            }
            break;
        default:
            break;
        }
        if (rgb_mode >= RGB_MODE_COUNT)
        {
            rgb_mode = 0;
        }
        interval_cnt = 25;
    }
    if (interval_cnt > 0)
    {
        interval_cnt--;
    }
}
void rgb_mode_idle_fun(void)
{
    breathCnt++;
    if (breathCnt >= breathTime)
    {
        breathCnt = 0;
    }
    if (breathCnt < (breathTime / 2))
    {
        r = color_list[0][0] * breathCnt / (breathTime / 2);
        g = color_list[0][1] * breathCnt / (breathTime / 2);
        b = color_list[0][2] * breathCnt / (breathTime / 2);
    }
    else
    {
        r = color_list[0][0] * (breathTime - breathCnt) / (breathTime / 2);
        g = color_list[0][1] * (breathTime - breathCnt) / (breathTime / 2);
        b = color_list[0][2] * (breathTime - breathCnt) / (breathTime / 2);
    }
    // all rgb is white
    mainRgb[0] = r;
    mainRgb[1] = g;
    mainRgb[2] = b;

    tailRgb[0] = r;
    tailRgb[1] = g;
    tailRgb[2] = b;
}
void rgb_mode_1_fun(void)
{
    // 主灯七彩闪烁，尾灯长灯
    breathCnt++;
    if (breathCnt >= breathTime)
    {
        breathCnt = 0;
        flagCnt++;
        if (flagCnt >= COLOR_NUM)
        {
            flagCnt = 0;
        }
    }
    if (breathCnt < (breathTime / 2))
    {
        r = color_list[flagCnt][0] * breathCnt / (breathTime / 2);
        g = color_list[flagCnt][1] * breathCnt / (breathTime / 2);
        b = color_list[flagCnt][2] * breathCnt / (breathTime / 2);
    }
    else
    {
        r = color_list[flagCnt][0] * (breathTime - breathCnt) / (breathTime / 2);
        g = color_list[flagCnt][1] * (breathTime - breathCnt) / (breathTime / 2);
        b = color_list[flagCnt][2] * (breathTime - breathCnt) / (breathTime / 2);
    }
    mainRgb[0] = r;
    mainRgb[1] = g;
    mainRgb[2] = b;

    tailRgb[0] = color_list[rgb_Index][0];
    tailRgb[1] = color_list[rgb_Index][1];
    tailRgb[2] = color_list[rgb_Index][2];
}
void rgb_mode_2_fun(void)
{
    // 主灯长亮，尾灯长灯
    breathCnt++;
    if (breathCnt >= breathTime)
    {
        breathCnt = 0;
    }
    if (breathCnt < (breathTime / 2))
    {
        r = color_list[rgb_Index][0] * breathCnt / (breathTime / 2);
        g = color_list[rgb_Index][1] * breathCnt / (breathTime / 2);
        b = color_list[rgb_Index][2] * breathCnt / (breathTime / 2);
    }
    else
    {
        r = color_list[rgb_Index][0] * (breathTime - breathCnt) / (breathTime / 2);
        g = color_list[rgb_Index][1] * (breathTime - breathCnt) / (breathTime / 2);
        b = color_list[rgb_Index][2] * (breathTime - breathCnt) / (breathTime / 2);
    }
    mainRgb[0] = r;
    mainRgb[1] = g;
    mainRgb[2] = b;

    tailRgb[0] = r;
    tailRgb[1] = g;
    tailRgb[2] = b;
}

/**
 * @brief 黄灯慢闪
 *
 */
void rgb_rc_lost(void)
{
    breathCnt++;
    if (breathCnt >= breathTime)
    {
        breathCnt = 0;
    }
    if (breathCnt < (breathTime / 2))
    {
        r = 255;
        g = 255;
        b = 0;
    }
    else
    {
        r = 0;
        g = 0;
        b = 0;
    }
    mainRgb[0] = r;
    mainRgb[1] = g;
    mainRgb[2] = b;

    tailRgb[0] = r;
    tailRgb[1] = g;
    tailRgb[2] = b;
}
/**
 * @brief 红灯慢闪
 *
 */
void rgb_pwr_low()
{
    breathCnt++;
    if (breathCnt >= breathTime)
    {
        breathCnt = 0;
    }
    if (breathCnt < (breathTime / 2))
    {
        r = 255;
        g = 0;
        b = 0;
    }
    else
    {
        r = 0;
        g = 0;
        b = 0;
    }
    mainRgb[0] = r;
    mainRgb[1] = g;
    mainRgb[2] = b;

    tailRgb[0] = r;
    tailRgb[1] = g;
    tailRgb[2] = b;
}
/**
 * @brief 绿灯快闪
 *
 */
void rgb_arming_alert(void)
{
    breathCnt++;
    if (breathCnt >= breathTime)
    {
        breathCnt = 0;
    }
    if (breathCnt < (breathTime / 2))
    {
        r = 0;
        g = 255;
        b = 0;
    }
    else
    {
        r = 0;
        g = 0;
        b = 0;
    }
    mainRgb[0] = r;
    mainRgb[1] = g;
    mainRgb[2] = b;

    tailRgb[0] = r;
    tailRgb[1] = g;
    tailRgb[2] = b;
}
/**
 * @brief 白灯快闪
 *
 */
void rgb_hardfault()
{
    breathCnt++;
    if (breathCnt >= breathTime)
    {
        breathCnt = 0;
    }
    if (breathCnt < (breathTime / 2))
    {
        r = 255;
        g = 255;
        b = 255;
    }
    else
    {
        r = 0;
        g = 0;
        b = 0;
    }

    mainRgb[0] = r;
    mainRgb[1] = g;
    mainRgb[2] = b;

    tailRgb[0] = r;
    tailRgb[1] = g;
    tailRgb[2] = b;
}
/**
 * @brief 蓝灯快闪
 *
 */
void rgb_manual_mode_fun(void)
{
    static uint8_t manaulCnt;
    manaulCnt++;
    if (manaulCnt >= breathTime)
    {
        manaulCnt = 0;
    }
    if (manaulCnt < (breathTime / 2))
    {
        r = 0;
        g = 0;
        b = 255;
    }
    else
    {
        r = 0;
        g = 0;
        b = 0;
    }
    manaulModeRgb[0] = r;
    manaulModeRgb[1] = g;
    manaulModeRgb[2] = b;
}
void rgb_fliting_mode_fun(void)
{
    static uint8_t flitCnt;
    flitCnt++;
    if (flitCnt >= breathTime)
    {
        flitCnt = 0;
    }
    if (flitCnt < (breathTime / 2))
    {
        r = 255;
        g = 0;
        b = 0;
    }
    else
    {
        r = 0;
        g = 0;
        b = 0;
    }
    flitingRgb[0] = r;
    flitingRgb[1] = g;
    flitingRgb[2] = b;
}
void rgb_switch(bool triger)
{
    rgb_mode_switch_fun(triger);

     if(FC_ERROR_CHECK(FC_RC_HARDFAULT) || FC_ERROR_CHECK(FC_ACC_GYRO_HARDFAULT) || FC_ERROR_CHECK(FC_BARO_HARDFAULT))
    {
        breathTime = 20;
        rgb_hardfault();
    }
    else if (FC_ERROR_CHECK(FC_LOW_POWER))
    {
        breathTime = 50;
        rgb_pwr_low();
    }
    else if (FC_ERROR_CHECK(FC_RC_LOST))
    {
        breathTime = 50;
        rgb_rc_lost();
    }
    else if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
    {
        breathTime = 20;
        rgb_pwr_low();
    }
    else if (FC_ERROR_CHECK(FC_ARMING_ALERK) || FC_ERROR_CHECK(FC_OK_TO_ARM))
    {
        breathTime = 20;
        rgb_arming_alert();
    }

    else
    {
        switch (rgb_mode)
        {
        case RGB_MODE_IDLE:
            breathTime = 100;
            rgb_mode_idle_fun();
            break;
        case RGB_MODE_1:
            breathTime = 26;
            rgb_mode_1_fun();
            break;
        case RGB_MODE_2:
            breathTime = 100;
            rgb_mode_2_fun();
            break;
        default:
            break;
        }
    }
    // rt_kprintf("rgb_mode:%d, rgb_Index:%d flagCnt:%d breathCnt:%d/%d\n", rgb_mode, rgb_Index, flagCnt,breathCnt,breathTime);
    rgb_set_color(0, mainRgb[0], mainRgb[1], mainRgb[2]);
    // rgb_set_color(1, mainRgb[0], mainRgb[1], mainRgb[2]);
    if (FC_ERROR_CHECK(FC_MANUAL_MODE))
    {
        breathTime = 30;
        rgb_manual_mode_fun();
        rgb_set_color(1, manaulModeRgb[0], manaulModeRgb[1], manaulModeRgb[2]);
    }
    else if(FC_ERROR_CHECK(FC_FLIPING_MODE))
    {
        breathTime = 20;
        rgb_fliting_mode_fun();
        rgb_set_color(1, flitingRgb[0], flitingRgb[1], flitingRgb[2]);
    }
    else
    {
        rgb_set_color(1, tailRgb[0], tailRgb[1], tailRgb[2]);
    }
    rgb_set_color(2, tailRgb[0], tailRgb[1], tailRgb[2]);
    rgb_set_color(3, tailRgb[0], tailRgb[1], tailRgb[2]);

    if((mixlyRgbMode1 != MIXLY_RGB_MODE_IDLE) || (mixlyRgbMode2 != MIXLY_RGB_MODE_IDLE))
    {
        mixly_rgb_mode_handle();
    }

    rgb_show();
}


void generate_led_blink_value()
{
    static uint8_t ledCnt = 0;
    ledCnt++;
    if (ledCnt > SHOW_TIME) {
        ledCnt = 0;
    }

    rt_memset(mixlyBlinkTable, 0, sizeof(mixlyBlinkTable));

    if ((ledCnt % (SHOW_TIME / 4)) < (SHOW_TIME / 8)) {
        mixlyBlinkTable[MIXLY_RGB_FAST] = MAX_DIN;
    }
    if (ledCnt < (SHOW_TIME / 2)) {
        mixlyBlinkTable[MIXLY_RGB_SLOW] = MAX_DIN;
    }

    if (ledCnt < (SHOW_TIME / 2)) {
        mixlyBlinkTable[MIXLY_RGB_BREATH] = MAX_DIN * ledCnt / (SHOW_TIME / 2);
    } else {
        mixlyBlinkTable[MIXLY_RGB_BREATH] = MAX_DIN * (SHOW_TIME - ledCnt) / (SHOW_TIME / 2);
    }

    if (ledCnt < (SHOW_TIME / 3)) {
        if ((ledCnt % (SHOW_TIME / 3 / 4)) == 0) {
            mixlyBlinkTable[MIXLY_RGB_DOT] = MAX_DIN;
        }
    }
}

void set_rgb_mode(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t m1, uint8_t r2, uint8_t g2, uint8_t b2, uint8_t m2)
{
    mixlyRgbMode1 = m1;
    mixlyRgbMode2 = m2;
    mixlyMainRgb[0] = r1;
    mixlyMainRgb[1] = g1;
    mixlyMainRgb[2] = b1;
    mixlyTailRgb[0] = r2;
    mixlyTailRgb[1] = g2;
    mixlyTailRgb[2] = b2;
}
void mixly_rgb_mode_handle(void)
{
    uint8_t r = 0, g = 0, b = 0;

    generate_led_blink_value();

    r = mixlyBlinkTable[mixlyRgbMode1] * mixlyMainRgb[0] / MAX_DIN;
    g = mixlyBlinkTable[mixlyRgbMode1] * mixlyMainRgb[1] / MAX_DIN;
    b = mixlyBlinkTable[mixlyRgbMode1] * mixlyMainRgb[2] / MAX_DIN;
    rgb_set_color(0, r, g, b);

    r = mixlyBlinkTable[mixlyRgbMode2] * mixlyTailRgb[0] / MAX_DIN;
    g = mixlyBlinkTable[mixlyRgbMode2] * mixlyTailRgb[1] / MAX_DIN;
    b = mixlyBlinkTable[mixlyRgbMode2] * mixlyTailRgb[2] / MAX_DIN;
    rgb_set_color(1, r, g, b);
    rgb_set_color(2, r, g, b);
    rgb_set_color(3, r, g, b);
}
